/*
 * Copyright (C) 2006 Roman Krashanitsa
 *
 * @author Roman Krashanitsa
 *
 * This file is part of paparazzi.
 *
 * paparazzi is free software; you can redistribute it and/or modify
 * it under the terms of the GNU General Public License as published by
 * the Free Software Foundation; either version 2, or (at your option)
 * any later version.
 *
 * paparazzi is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 * GNU General Public License for more details.
 *
 * You should have received a copy of the GNU General Public License
 * along with paparazzi; see the file COPYING.  If not, write to
 * the Free Software Foundation, 59 Temple Place - Suite 330,
 * Boston, MA 02111-1307, USA. 
 *
 */

package pprz.data_components;

import java.util.HashMap;
import javax.swing.event.EventListenerList;

import pprz.data_components.events.DownlinkMessageEvent;
import pprz.data_components.events.DownlinkMessageEventListener;
import pprz.data_components.parsers.MessageParser.MessageFormat;

import net.jscience.math.MathFP;


public class SimPprzMsgProtocol extends MsgProtocol implements Runnable{
	
	//protected EventListenerList listenerList;
	//protected DownlinkMessageEvent e;
	protected HashMap<Integer,Aircraft> telemetrySenders;
	HashMap<Long,MessageFormat> formats;
	protected MessageFormat format;
	protected Message message;
	protected Thread t;
	protected int max;
	protected double start_lon;
	protected double start_lat;
	protected short sim_msg[]={6,8,11,12,23,10};
	protected short sim_acs[]={10,32,36};
	protected int sim_colors[]={0xFF0000,0x00FF00,0x0000FF};
	
	
	public SimPprzMsgProtocol()
	{
		super();
		message=new Message();
		t=new Thread(this);
	}
	
	public void init(HashMap<Integer,Aircraft> senderMap, HashMap<Long,MessageFormat> m_format,
			 double start_lon, double start_lat)
	{
		telemetrySenders=senderMap;
		formats=m_format;		
		max=sim_acs.length;
		this.start_lon=start_lon;
		this.start_lat=start_lat;
		t.start();		
	}
	
	public void run() {
		try {
			Thread.sleep(1000);
		} catch (InterruptedException e)
		{}
		while (true) {
		int id=sim_acs[(int)(Math.round(Math.random()*(sim_acs.length-1)))]; //sender id
		int msgid=sim_msg[(int)(Math.round(Math.random()*(sim_msg.length-1)))]; //message type
		Aircraft a;
		if (!(telemetrySenders.containsKey(id))&& telemetrySenders.size()<max) {
			int n=(int)(Math.random()*256)<<16<<16+(int)(Math.random()*256)<<16+(int)(Math.random()*256);
			int p=n;
			telemetrySenders.put(id, new Aircraft(id,(int)(Math.random()*256)<<16<<16+(int)(Math.random()*256)<<16+(int)(Math.random()*256)));
		}
		else if (!telemetrySenders.containsKey(id))
		{
			while (!(telemetrySenders.containsKey(id))) { 
				id=sim_acs[(int)(Math.random()*(sim_acs.length-1))]; //sender id
			}
		} 
		assert(id!=0);
		a=telemetrySenders.get(id);
		a.lastMSG=msgid;
		if (!formats.containsKey(0*0x100000000l+msgid)) return;
		
		format=formats.get(0*0x100000000l+msgid);
		if (format.noName || format.noClass || format.noId || 
			format.noTypeField>0 || format.noNameField>0) return;
		//else format is ok --->
		message.setFieldsLength(format.fields.size());
		//System.out.printf("Sent msgid=%d,n=%d\n",msgid,format.fields.size());
		Long[] fld;
		switch (msgid) {
			case 6: {
				//set roll, heading, pitch
				a.set_roll((int)(Math.random()*180-90));
				a.set_yaw((int)(Math.random()*180-90));
				a.set_pitch((int)(Math.random()*180-90));
				message.setId(6);
				fld=new Long[1];fld[0]=MathFP.toFP((int)a.get_roll()); message.setField(0, fld);
				fld=new Long[1];fld[0]=MathFP.toFP((int)a.get_gps_course()); message.setField(1, fld);
				fld=new Long[1];fld[0]=MathFP.toFP((int)a.get_pitch()); message.setField(2, fld);
				break;
			}
			case 8: {
				a.set_gps_alt((int)(Math.random()*60+75000)); // altitude, cm
				a.set_gps_vel((int)(Math.random()*500+1000)); // speed, cm/s
				a.set_gps_climb((int)(Math.random()*200)); // climb, cm/s
				a.set_gps_utm_north((long)((start_lat+Math.random()*0.001)*100000)); // latitude, cm
				a.set_gps_utm_east((long)((start_lon+Math.random()*0.001)*100000)); // longitude, cm
				a.updateTraj();
				message.setId(8);
				fld=new Long[1];fld[0]=MathFP.toFP(0); message.setField(0, fld);
				fld=new Long[1];fld[0]=MathFP.toFP((long)a.get_gps_utm_east()); message.setField(1, fld);				
				fld=new Long[1];fld[0]=MathFP.toFP((long)a.get_gps_utm_north()); message.setField(2, fld);
				fld=new Long[1];fld[0]=MathFP.toFP(0); message.setField(3, fld);
				fld=new Long[1];fld[0]=MathFP.toFP((long)a.get_gps_alt()); message.setField(4, fld);
				fld=new Long[1];fld[0]=MathFP.toFP((long)a.get_gps_vel()); message.setField(5, fld);
				fld=new Long[1];fld[0]=MathFP.toFP((long)a.get_gps_climb()); message.setField(6, fld);
				fld=new Long[1];fld[0]=MathFP.toFP(0); message.setField(7, fld);
				fld=new Long[1];fld[0]=MathFP.toFP(0); message.setField(8, fld);
				fld=new Long[1];fld[0]=MathFP.toFP(0); message.setField(9, fld);
				break;				
			}
			case 11: {
				a.set_ap_mode((byte)(Math.random()*2));
				message.setId(11);
				fld=new Long[1];fld[0]=MathFP.toFP(a.get_ap_mode()); message.setField(0, fld);
				fld=new Long[1];fld[0]=MathFP.toFP(0); message.setField(1, fld);
				fld=new Long[1];fld[0]=MathFP.toFP(0); message.setField(2, fld);
				fld=new Long[1];fld[0]=MathFP.toFP(0); message.setField(3, fld);
				fld=new Long[1];fld[0]=MathFP.toFP(0); message.setField(4, fld);
				fld=new Long[1];fld[0]=MathFP.toFP(0); message.setField(5, fld);
				fld=new Long[1];fld[0]=MathFP.toFP(0); message.setField(6, fld);
				break;				
			}
			case 12: {
				a.set_throttle((float)Math.random());
				a.set_voltage((float)(Math.random()*6+7)); 
				a.set_flight_time((float)(Math.random()*1000));
				message.setId(12);
				fld=new Long[1];fld[0]=MathFP.toFP((long)(a.get_throttle()*100)); message.setField(0, fld);
				fld=new Long[1];fld[0]=MathFP.toFP((long)(a.get_voltage()*10)); message.setField(1, fld);
				fld=new Long[1];fld[0]=MathFP.toFP((long)(a.get_flight_time()*100)); message.setField(2, fld);
				fld=new Long[1];fld[0]=MathFP.toFP(0); message.setField(3, fld);
				fld=new Long[1];fld[0]=MathFP.toFP(0); message.setField(4, fld);
				fld=new Long[1];fld[0]=MathFP.toFP(0); message.setField(5, fld);
				fld=new Long[1];fld[0]=MathFP.toFP(0); message.setField(6, fld);
				break;				
			}
			case 23: {
				a.set_rx_bytes_rate((float)(200+Math.random()*100));
				a.set_rx_msgs_rate((float)(25+Math.random()*10));
				message.setId(23);
				fld=new Long[1];fld[0]=MathFP.toFP(0); message.setField(0, fld);
				fld=new Long[1];fld[0]=MathFP.toFP(0); message.setField(1, fld);
				fld=new Long[1];fld[0]=MathFP.toFP(0); message.setField(2, fld);
				fld=new Long[1];fld[0]=MathFP.toFP(0); message.setField(3, fld);
				fld=new Long[1];fld[0]=MathFP.toFP((int)(a.get_rx_bytes_rate()*100)); message.setField(4, fld);
				fld=new Long[1];fld[0]=MathFP.toFP((int)(a.get_rx_msgs_rate()*100)); message.setField(5, fld);
				break;				
			}
			case 10: {
				//a.setByteRate((float)(200+Math.random()*100));
				//a.setMsgRate((float)(25+Math.random()*10));
				//a.state=Aircraft.RATE;
				message.setId(10);
				fld=new Long[1];fld[0]=MathFP.toFP((long)(1)); message.setField(0, fld);
				fld=new Long[1];fld[0]=MathFP.toFP((long)(0)); message.setField(1, fld);
				fld=new Long[1];fld[0]=MathFP.toFP((long)(Math.random()*1000)); message.setField(2, fld);
				fld=new Long[1];fld[0]=MathFP.toFP((long)(Math.random()*1000)); message.setField(3, fld);
				fld=new Long[1];fld[0]=MathFP.toFP((long)(Math.random()*1000)); message.setField(4, fld);
				fld=new Long[1];fld[0]=MathFP.toFP((long)(Math.random()*100)); message.setField(5, fld);
				fld=new Long[1];fld[0]=MathFP.toFP((long)(Math.random()*100)); message.setField(6, fld);
				break;				
			}
			default: {
				break;
			}
		}
		message.setClass((byte)0);
		message.setTime(System.currentTimeMillis());
		message.setPayload(a);
		fireDownlinkMessageEvent(new DownlinkMessageEvent(this,message));
		try {
			Thread.sleep(10);
		} catch (InterruptedException e)
		{System.out.println("Sim interrupted.");}
		}
	}		
}
